#include <stdio.h>
#include <cmath>
#include "structure.h"
#include "cv.h"

// ポイントマップの読み込み
int load_point_map(const char *filename, PointMap *map, double scale)
{
  FILE *fp;
  int i;
  char tmp[256];
  Point p1;

  if((fp=fopen(filename, "r"))==NULL){
    perror("ERROR: load_point_map");
    return -1;
  }
  fprintf(stderr,"loading pointmap \"%s\"...",  filename);
  for (i=0; (i < MAX_POINTMAP_SIZE) && (fgets(tmp,256,fp)!=NULL) ; i++) {
    sscanf(tmp, "%lf\t%lf\n", 
	   &(p1.x), &(p1.y));
    map->data[i].x = p1.x*scale;
    map->data[i].y = p1.y*scale;
  }
  fprintf(stderr," %d points loaded.\n",i);
  fclose(fp);

  map->numdata = i;

  return 0;
}

// ラインマップの読み込み
int load_line_map(const char *filename, LineMap *map, double scale)
{
  FILE *fp;
  int i;
  char tmp[256];
  Point p1,p2;

  if((fp=fopen(filename, "r"))==NULL){
    perror("ERROR: load_line_map");
    return -1;
  }
  fprintf(stderr,"loading linemap \"%s\"...",  filename);
  for (i=0; (i < MAX_LINEMAP_SIZE) && (fgets(tmp,256,fp)!=NULL) ; i++) {
    sscanf(tmp, "%lf\t%lf\t%lf\t%lf\n", 
	   &(p1.x), &(p1.y), 
	   &(p2.x), &(p2.y));
    map->data[i].p1.x = p1.x*scale;
    map->data[i].p1.y = p1.y*scale;
    map->data[i].p2.x = p2.x*scale;
    map->data[i].p2.y = p2.y*scale;
  }
  fprintf(stderr," %d lines loaded.\n",i);
  fclose(fp);

  map->numdata = i;

  return 0;
}

// ラインマップをポイントマップに変換
void convert_linemap_to_pointmap(LineMap *map, PointMap *pmap, double resolution){
  int i,j;
  //double resolution  = 7.0/100.0; // [m/grid]
  Point p1,p2;
  int steps;
  int idx = 0;

  // y by x
  for(i = 0 ; i < map->numdata ; i++){
    double div;
    int sign;

    p1 = map->data[i].p1;
    p2 = map->data[i].p2;
    steps = fabs(p2.x - p1.x)/resolution;
    div = (p2.y - p1.y)/(p2.x -p1.x);
    sign = ((p1.x - p2.x)>0)? -1 : 1;
    for(j=0; j < steps ; j++){
      pmap->data[idx].x = (p1.x + sign*resolution*j);
      pmap->data[idx].y = (sign*div*resolution*j + p1.y);
      idx++;
    }
  }

  /*      // x by y */
  for(i = 0 ; i < map->numdata ; i++){
    double div;
    int sign;

    p1 = map->data[i].p1;
    p2 = map->data[i].p2;
    steps = fabs(p1.y - p2.y)/resolution;
    div = (p2.x - p1.x)/(p2.y -p1.y);
    sign = ((p2.y-p1.y)>0)? 1: -1;

    for(j=0; j < steps ; j++){
      pmap->data[idx].x = (div*sign*resolution*j + p1.x);
      pmap->data[idx].y = (p1.y + sign*resolution*j);

      idx++;
    }
  }

  pmap->numdata = idx;

  fprintf(stderr," %d points converted.\n",idx);
}

// 占有グリッドマップの読み込み

int load_occupancy_grid_map(const char *filename, OccupancyGridMap *map)
{
  FILE *fp;
  char buf[255];
  int i;

  map->width = 0;
  map->height = 0;
  map->resolution = 0;

  // ファイルのオープン 

  if ((fp = fopen(filename, "rb")) == NULL) {
    fprintf(stderr, "can't open file(%s)\n", filename);
    return -1;
  }

  //画像形式のチェック

  if (fgets(buf, 255, fp) == NULL) {
    fprintf(stderr, "can't read ppm file...\n");
    return -1;
  }
  if (strncmp(buf, "P5", 2)){
    fprintf(stderr, "%s isnot a pgm image of binary\n", filename);
    return -1;
  }

  do {
    if (fgets(buf, 255, fp) == NULL) {
      fprintf(stderr, "can't read pgm file...\n");
      return -1;
    }else{

      // 解像度の取得

      if(strncmp(buf, "#resolution", 11) == 0){
	char dummy[100];
	sscanf(buf, "%s %lf", dummy, &map->resolution);
      }
    }

	// コメントの読み飛ばし

  }while (!strncmp(buf, "#", 1));

  // サイズの確認

  sscanf(buf, "%d %d", &map->width, &map->height);
  if((map->width * map->height) > MAX_OCCUPANCY_GRIDMAP_SIZE){
    fprintf(stderr, "this map is too large...\n");
    return -1;
  }

  //階調の読み飛ばし

  if (fgets(buf, 255, fp) == NULL) {
    fprintf(stderr, "can't read pgm file...\n");
    return -1;
  }

  // 画像データの読み込み (Y座標を上下反転)

  for(i=0; i<map->height; i++){
    if (fread(&map->data[map->width*(map->height-1-i)], sizeof(unsigned char), map->width, fp) != (unsigned int)map->width) {
      fprintf(stderr, "can't read PGM file...\n");
      return -1;
    }
  }

  fprintf(stderr," %dx%d image loaded.\n",map->width, map->height);

  // ファイルのクローズ

  fclose(fp);

  return 0;  
}

// 占有グリッドマップをポイントマップに変換

void convert_occupancy_gridmap_to_pointmap(OccupancyGridMap *ogmap, PointMap *pmap, unsigned char threshold)
{
  int i;
  int idx=0;
  int w = ogmap->width;
  int h = ogmap->height;

  IplImage *dst = cvCreateImage(cvSize(w,h), IPL_DEPTH_8U, 1);
  IplImage *src = cvCreateImageHeader(cvSize(w,h), IPL_DEPTH_8U, 1);
  src->imageData = (char *)ogmap->data;

  cvThreshold(src, dst, threshold, 255, CV_THRESH_BINARY);
  cvCanny(dst, dst, 50, threshold);

  for(i=0; i<(dst->width*dst->height); i++){
    int ix = i%w;
    int iy = i/w;

    // 輝度値が一定以上（エッジ検出時に白黒反転している）

    if(dst->imageData[i]){
      pmap->data[idx].x = (int)ix * ogmap->resolution;
      pmap->data[idx].y = (int)iy * ogmap->resolution;
      ++idx;
    }
  }

  pmap->numdata = idx;

  cvReleaseImageHeader( &src );
  cvReleaseImage( &dst );
  fprintf(stderr," %d points converted.\n",idx);
}
